/*
 * Copyright 2016 The Cartographer Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef CARTOGRAPHER_MAPPING_LOCAL_TRAJECTORY_BUILDER_2D_H_
#define CARTOGRAPHER_MAPPING_LOCAL_TRAJECTORY_BUILDER_2D_H_

#include <chrono>
#include <memory>

#include "common/time.h"
#include "mapping/submap_2d.h"
#include "scan_matching/ceres_scan_matcher_2d.h"
#include "scan_matching/real_time_correlative_scan_matcher_2d.h"
#include "mapping/motion_filter.h"
#include "mapping/range_data_collator.h"
#include "mapping/pose_extrapolator.h"
#include "sensor/imu_data.h"
#include "sensor/voxel_filter.h"
#include "sensor/odometry_data.h"
#include "sensor/range_data.h"
#include "sensor/timed_point_cloud_data.h"

#include "transform/rigid_transform.h"

#include "publisher/publisher.h"

namespace cartographer {
namespace mapping {
struct Options {
    bool use_imu_data_ = false;            
    double min_range_ = 0.0;                 
    double max_range_ = 50.;
    double min_z_ = -0.8;                 
    double max_z_ = 2.0;
    double missing_data_ray_length_ = 5.0;   
    int num_accumulated_range_data_ = 1;
    double voxel_filter_size_ = 0.025;    

    double voxel_filter_max_length_ = 0.5;             
    int voxel_filter_min_num_points_ = 200;        
    double voxel_filter_max_range_ = 50.0;   

    bool use_online_correlative_scan_matching_ = false;

    PoseExtrapolatorOptions pose_extrapolator_options_;    
    PoseExtrapolatorOptions pose_extrapolator_options() {
      return pose_extrapolator_options_;
    }   

    bool use_imu_data() {
      return use_imu_data_;
    } 
    double min_range() {
      return min_range_;
    }

    double max_range() {
      return max_range_;
    }
    double min_z() {
      return min_z_;
    }
    double max_z() {
      return max_z_;
    }    
    double missing_data_ray_length() {
      return missing_data_ray_length_;
    }  

    int num_accumulated_range_data() {
      return num_accumulated_range_data_;
    }  
    double voxel_filter_size() {
      return voxel_filter_size_;
    }

    bool use_online_correlative_scan_matching() {
      return use_online_correlative_scan_matching_;
    }
};  

// Wires up the local SLAM stack (i.e. pose extrapolator, scan matching, etc.)
// without loop closure.
// TODO: Add test for this class similar to the 3D test.
class LocalTrajectoryBuilder2D {
 public:
  // 将点云插入到地图后的result
  struct InsertionResult {
    std::shared_ptr<const TrajectoryNode::Data> constant_data;
    std::vector<std::shared_ptr<const Submap2D>> insertion_submaps; // 最多只有2个子图的指针
  };
  // 扫描匹配的result
  struct MatchingResult {
    common::Time time;
    transform::Rigid3d local_pose;
    sensor::RangeData range_data_in_local; // 经过扫描匹配之后位姿校准之后的雷达数据
    // 'nullptr' if dropped by the motion filter.
    std::unique_ptr<const InsertionResult> insertion_result;
  };

  LocalTrajectoryBuilder2D(const std::vector<std::string>& expected_range_sensor_ids, std::shared_ptr<Publisher> publisher_ptr = nullptr);
  ~LocalTrajectoryBuilder2D();

  LocalTrajectoryBuilder2D(const LocalTrajectoryBuilder2D&) = delete;
  LocalTrajectoryBuilder2D& operator=(const LocalTrajectoryBuilder2D&) = delete;

  // Returns 'MatchingResult' when range data accumulation completed,
  // otherwise 'nullptr'. Range data must be approximately horizontal
  // for 2D SLAM. `TimedPointCloudData::time` is when the last point in
  // `range_data` was acquired, `TimedPointCloudData::ranges` contains the
  // relative time of point with respect to `TimedPointCloudData::time`.
  std::unique_ptr<MatchingResult> AddRangeData(
      const std::string& sensor_id,
      const sensor::TimedPointCloudData& range_data);
  void AddImuData(const sensor::ImuData& imu_data);
  void AddOdometryData(const sensor::OdometryData& odometry_data);

 private:
  std::unique_ptr<MatchingResult> AddAccumulatedRangeData(
      common::Time time, const sensor::RangeData& gravity_aligned_range_data,
      const transform::Rigid3d& gravity_alignment);
  
  sensor::RangeData TransformToGravityAlignedFrameAndFilter(
      const transform::Rigid3f& transform_to_gravity_aligned_frame,
      const sensor::RangeData& range_data);

  std::unique_ptr<InsertionResult> InsertIntoSubmap(
      common::Time time, const sensor::RangeData& range_data_in_local,
      const sensor::PointCloud& filtered_gravity_aligned_point_cloud,
      const transform::Rigid3d& pose_estimate,
      const Eigen::Quaterniond& gravity_alignment);

  // Scan matches 'filtered_gravity_aligned_point_cloud' and returns the
  // observed pose, or nullptr on failure.
  std::unique_ptr<transform::Rigid2d> ScanMatch(
      common::Time time, const transform::Rigid2d& pose_prediction,
      const sensor::PointCloud& filtered_gravity_aligned_point_cloud);

  // Lazily constructs a PoseExtrapolator.
  void InitializeExtrapolator(common::Time time);

  ActiveSubmaps2D active_submaps_;

  MotionFilter motion_filter_;
  scan_matching::RealTimeCorrelativeScanMatcher2D real_time_correlative_scan_matcher_;
  scan_matching::CeresScanMatcher2D ceres_scan_matcher_;

  std::unique_ptr<PoseExtrapolator> extrapolator_;

  int num_accumulated_ = 0;
  sensor::RangeData accumulated_range_data_;


  RangeDataCollator range_data_collator_;

  Options options_;

  std::shared_ptr<Publisher> publisher_ptr_ = nullptr;
};

}  // namespace mapping
}  // namespace cartographer

#endif  // CARTOGRAPHER_MAPPING_INTERNAL_2D_LOCAL_TRAJECTORY_BUILDER_2D_H_
